#include "SketchWidget.h"
#include <QDebug>
#include <QMouseEvent>
#include <QtCore/qmath.h>
#include "math.h"
#include "window.h"

#define PI 3.14159265

SketchWidget::SketchWidget(QWidget *parent) :
    QGLWidget(parent)
{
    selectedPoint = -1;
    b_move = false;
    mode = nothing;

    oldAngleRotate = 0;
    angleRotate = 0;

    viewDirectionLeft = rotatePoint(Point(0.0, 0.40),23.45 * PI / 180);
    viewDirectionLeft.limit(0.6);
    viewDirectionRight = rotatePoint(Point(0.0, 0.40),-23.45 * PI / 180);
    viewDirectionRight.limit(0.6);

    //rotateFov(-PI/3);

    setMinimumSize(400, 600);
    addPoint(Point(0, -0.7));
}

void SketchWidget::initializeGL()
{
    setMouseTracking(true);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnable(GL_DEPTH_TEST);
    glClearColor(0., 0., 0., 0.);
}

void SketchWidget::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glClearColor(1., 1., 1., 1.);

    glEnable(GL_DEPTH_TEST);
    glEnable(GL_DOUBLEBUFFER);
    glEnable(GL_POINT_SMOOTH);
    glEnable(GL_LINE_SMOOTH);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glPushMatrix();

    if ( mode == rotate) {
        drawCircleRotation();
        drawOldPath();
    } else if ( mode == inRotation ) {
        drawOldPath();
        drawTempPath();
    } else if ( mode == deleteOrAddPoint && b_move) {
        drawEraserLine();
    }
    drawRobot();
    drawPath();
    drawGrid();
    drawFov();

    glPopMatrix();
}

void SketchWidget::resizeGL(int w, int h)
{
    glViewport(0, 0, w, h);
    updateGL();
}

void SketchWidget::mousePressEvent(QMouseEvent* evt)
{
    if ( evt->button() == Qt::LeftButton ) {
        Point mouse = mouseTo2D(evt->x(), evt->y());

       int nearest = isNearPoint(mouse);
       if ( mode == rotate ) {
           qDebug("angle: %f", angleRotate*180/PI);

           mode = inRotation;

           //We send instruction to the robot
           mainWindow->doBodyRotation(angleRotate);

           oldAngleRotate = 0;
           angleRotate = 0;
           mouseRotate = 0;

       } else if ( nearest != -1 && nearest != 0) {
            selectedPoint = nearest;
            mode = moveOrChangePoint;
        } else if (isOnRobotBody(mouse)) {
            mode = rotate;
            m_oldPoints = m_points;
        } else {
            mode = deleteOrAddPoint;
            p1EraserLine = mouse;
        }
        updateGL();
    }
}

void SketchWidget::mouseMoveEvent(QMouseEvent* evt)
{
    Point mouse = mouseTo2D(evt->x(), evt->y());
    if (mode == moveOrChangePoint) {
        m_points[selectedPoint] = mouse;
        b_move = true;
    } else if (mode == deleteOrAddPoint) {
        p2EraserLine = mouse;
        b_move = true;
    } else if (mode == rotate) {
        mouseRotate = mouse;
    }
    updateGL();
}

void SketchWidget::mouseReleaseEvent(QMouseEvent* evt)
{
    if (evt->button() == Qt::LeftButton) {
        Point mouse = mouseTo2D(evt->x(), evt->y());
        qDebug("x: %f, y: %f", mouse.x(), mouse.y());
        if ( mode == deleteOrAddPoint && b_move) {
            deletePoint(p1EraserLine,mouse);
            mode = nothing;
            b_move = false;
            selectedPoint = -1;
        } else if ( mode == moveOrChangePoint && b_move) {
            mode = nothing;
            b_move = false;
            selectedPoint = -1;
        } else if ( mode == moveOrChangePoint) {
            changePointSpeed(selectedPoint);
            mode = nothing;
            selectedPoint = -1;
        } else if ( mode == deleteOrAddPoint ){
            addPoint(mouse);
            mode = nothing;

        }
        updateGL();
    }
}

void SketchWidget::drawGrid()
{
    double space = 0.25;

    for (double i = -1; i < 1; i += space) {
        glColor3f(0.5, 0.5, 0.5);
        glLineWidth(1.0);
        glBegin(GL_LINES);
        glVertex2d(- width() / 2, i);
        glVertex2d(width() / 2, i);
        glVertex2d(i, - height() / 2);
        glVertex2d(i, height() / 2);
        glEnd();
    }

}

void SketchWidget::drawPath()
{
    for (int i = 0; i < m_points.size() - 1; ++i) {
        switch(m_speeds[i]) {
        case SLOW:
            glColor3f(0.0, 1.0, 0.0);
            break;
        case NORMAL:
            glColor3f(0.0, 0.0, 1.0);
            break;
        case FAST:
            glColor3f(1.0, 0.0, 0.0);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_points[i].x(), m_points[i].y());
        glEnd();

        switch(m_speeds[i + 1]) {
        case SLOW:
            glColor3f(0.0, 1.0, 0.0);
            break;
        case NORMAL:
            glColor3f(0.0, 0.0, 1.0);
            break;
        case FAST:
            glColor3f(1.0, 0.0, 0.0);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_points[i + 1].x(), m_points[i + 1].y());
        glEnd();

        glColor3f(0.0, 0.0, 0.0);
        glLineWidth(2.0);
        glBegin(GL_LINES);
        glVertex2d(m_points[i].x(), m_points[i].y());
        glVertex2d(m_points[i + 1].x(), m_points[i + 1].y());
        glEnd();
    }
}

void SketchWidget::drawTempPath()
{
    for (int i = 0; i < m_points.size() - 1; ++i) {
        switch(m_speeds[i]) {
        case SLOW:
            glColor3f(0.67, 0.95, 0.67);
            break;
        case NORMAL:
            glColor3f(0.67, 0.67, 0.95);
            break;
        case FAST:
            glColor3f(0.95, 0.67, 0.67);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_points[i].x(), m_points[i].y());
        glEnd();

        switch(m_speeds[i + 1]) {
        case SLOW:
            glColor3f(0.67, 0.95, 0.67);
            break;
        case NORMAL:
            glColor3f(0.67, 0.67, 0.95);
            break;
        case FAST:
            glColor3f(0.95, 0.67, 0.67);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_points[i + 1].x(), m_points[i + 1].y());
        glEnd();

        glColor3f(0.67, 0.67, 0.67);
        glLineWidth(2.0);
        glBegin(GL_LINES);
        glVertex2d(m_points[i].x(), m_points[i].y());
        glVertex2d(m_points[i + 1].x(), m_points[i + 1].y());
        glEnd();
    }
}

void SketchWidget::drawOldPath()
{
    for (int i = 0; i < m_oldPoints.size() - 1; ++i) {
        switch(m_speeds[i]) {
        case SLOW:
            glColor3f(0.0, 1.0, 0.0);
            break;
        case NORMAL:
            glColor3f(0.0, 0.0, 1.0);
            break;
        case FAST:
            glColor3f(1.0, 0.0, 0.0);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_oldPoints[i].x(), m_oldPoints[i].y());
        glEnd();

        switch(m_speeds[i + 1]) {
        case SLOW:
            glColor3f(0.0, 1.0, 0.0);
            break;
        case NORMAL:
            glColor3f(0.0, 0.0, 1.0);
            break;
        case FAST:
            glColor3f(1.0, 0.0, 0.0);
            break;
        }
        glPointSize(20.0);
        glBegin(GL_POINTS);
        glVertex2d(m_oldPoints[i + 1].x(), m_oldPoints[i + 1].y());
        glEnd();

        glColor3f(0.0, 0.0, 0.0);
        glLineWidth(2.0);
        glBegin(GL_LINES);
        glVertex2d(m_oldPoints[i].x(), m_oldPoints[i].y());
        glVertex2d(m_oldPoints[i + 1].x(), m_oldPoints[i + 1].y());
        glEnd();
    }
}

void SketchWidget::drawRobot()
{
    //Body
    glColor3f(0.12, 0.56, 1.0);
    glBegin(GL_QUADS);
    glVertex2f(-0.15, -0.55);
    glVertex2f(0.15, -0.55);
    glVertex2f(0.15, -0.88);
    glVertex2f(-0.15, -0.88);
    glEnd();

    //Legs
    glPointSize(20.0);
    glBegin(GL_POINTS);
    glVertex2f(-0.14,-0.62);
    glVertex2f(0.14,-0.62);
    glVertex2f(-0.14,-0.82);
    glVertex2f(0.14,-0.82);
    glEnd();

    //Neck
    glBegin(GL_QUADS);
    glVertex2f(-0.04, -0.52);
    glVertex2f(-0.055, -0.55);
    glVertex2f(0.055, -0.55);
    glVertex2f(0.04, -0.52);
    glEnd();

    //Head
    glBegin(GL_TRIANGLES);
    glVertex2f(-0.09,-0.52);
    glVertex2f(0.09,-0.52);
    glVertex2f(0.0,-0.45);
    glEnd();
}

void SketchWidget::drawFov()
{

    Point center = Point(0.0, -0.53);

    glColor3f(1.0, 1.0, 0.4);
    glBegin(GL_TRIANGLES);
    glVertex2f(center.x() ,center.y());
    glVertex2f(viewDirectionLeft.x() + center.x(),viewDirectionLeft.y() + center.y());
    glVertex2f(viewDirectionRight.x() + center.x(),viewDirectionRight.y() + center.y());
    glEnd();
}

void SketchWidget::drawCircleRotation()
{
    Point pIn = pointOnCircleRotation(mouseRotate,0.18);
    Point pOut = pointOnCircleRotation(mouseRotate,0.24);
    glColor3f(1.0, 1.0, 1.0);
    glLineWidth(4.0);
    glBegin(GL_LINES);
    glVertex2d(pIn.x(), pIn.y());
    glVertex2d(pOut.x(), pOut.y());
    glEnd();

   //compute the angle
    oldAngleRotate = angleRotate;
    Point vectRef = Point(0.0, 0.30);
    Point vectAngle = Point(pOut.x(), pOut.y() + 0.70);

    angleRotate = angle(vectRef,vectAngle);

    doRotate(-oldAngleRotate);
    doRotate(angleRotate);

    drawTempPath();

    float angle;
    glColor3f(1.0, 0.0, 0.2);
    glBegin(GL_LINE_LOOP);
    for(int i = 0; i < 1000; i++) {
        angle = i*2*M_PI/1000;
        glVertex3f(0.0 + (cos(angle) * 0.18), -0.70 + (sin(angle) * 0.18), 0.0);
        glVertex3f(0.0 + (cos(angle) * 0.2), -0.70 + (sin(angle) * 0.2), 0.0);
        glVertex3f(0.0 + (cos(angle) * 0.22), -0.70 + (sin(angle) * 0.22), 0.0);
        glVertex3f(0.0 + (cos(angle) * 0.24), -0.70 + (sin(angle) * 0.24), 0.0);
    }
    glEnd();
}

void SketchWidget::drawEraserLine()
{
    glColor3f(1.0, 0.0, 0.0);
    glLineWidth(1.0);
    glBegin(GL_LINES);
    glVertex2d(p1EraserLine.x(), p1EraserLine.y());
    glVertex2d(p2EraserLine.x(), p2EraserLine.y());
    glEnd();
}

Point SketchWidget::mouseTo2D(int x, int y)
{
    return Point((double)x * 2.0 / (double)width() - 1.0,
                 (double)(height() - y) * 2.0 / (double)height() - 1.0);
}

void SketchWidget::addPoint(Point p)
{
    m_points.append(p);
    m_speeds.append(SLOW);
}

void SketchWidget::changePointSpeed(int i)
{
    switch(m_speeds[i]) {
    case SLOW:
        m_speeds[i] = NORMAL;
        break;
    case NORMAL:
        m_speeds[i] = FAST;
        break;
    case FAST:
        m_speeds[i] = SLOW;
        break;
    }
}

int SketchWidget::isNearPoint(Point p)
{
    int np = -1, i;
    float dx, dy, d, min = 10000.;
    for (i = 0; i < m_points.size(); i++)
    {
        dx = m_points.at(i).x() - p.x();
        dy = m_points.at(i).y() - p.y();
        d = qSqrt((dx * dx) + (dy * dy));
        if (d < min)
        {
            min = d;
            np = i;
        }
    }

    if (min > 0.068)
    {
        return -1;
    }
    else
    {
        return np;
    }
}

bool SketchWidget::isOnRobotBody(Point p)
{
    if ( p.x() >= -0.15 && p.x() <= 0.15 && p.y() <= -0.45 && p.y() >= -0.98) {
        return true;
    }  else {
        return false;
    }
}

bool SketchWidget::ccw(Point A, Point B, Point C)
{
    return (C.y()-A.y())*(B.x()-A.x()) > (B.y()-A.y())*(C.x()-A.x());
}

bool SketchWidget::intersect(Point A, Point B, Point C, Point D)
{
    return ( ccw(A,C,D) != ccw(B,C,D) && ccw(A,B,C) != ccw(A,B,D) );
}

void SketchWidget::deletePoint(Point p1, Point p2)
{
    for (int i=0; i < m_points.size()-1; i++) {
        if ( intersect(p1,p2,m_points[i],m_points[i+1]) ) {
            m_points.remove(i+1);
            m_speeds.remove(i+1);
            return;
        }
    }
}

Point SketchWidget::pointOnCircleRotation(Point A, float r)
{
    Point p;
    float x = A.x();
    float y = A.y() + 0.70;
    p.setX( (r*x)/sqrt(x*x + y*y));
    p.setY( ((r*y)/sqrt(x*x + y*y)) - 0.70);
    return p;
}

Point SketchWidget::translatePoint(Point p, double distance)
{
    Point temp;
    temp.setX( p.x() );
    temp.setY( p.y() - distance );
    return temp;
}


double SketchWidget::angle(Point v1, Point v2)
{
    //normalize
    Point tempv1 = Point(v1.x()/sqrtf(v1.x()*v1.x() + v1.y()*v1.y()), v1.y()/sqrtf(v1.x()*v1.x() + v1.y()*v1.y()));
    Point tempv2 = Point(v2.x()/sqrtf(v2.x()*v2.x() + v2.y()*v2.y()), v2.y()/sqrtf(v2.x()*v2.x() + v2.y()*v2.y()));

    double ps = tempv1.x()*tempv2.x() + tempv1.y()*tempv2.y();

    if (v2.x() >= 0 ) {
        return acosf(ps);
    } else {
        return -acosf(ps);
    }
}

Point SketchWidget::rotatePoint(Point p, double angle)
{
   Point temp;
   temp.setX( p.x()*cos(angle) - (p.y()+0.70)*sin(angle) );
   temp.setY( p.x()*sin(angle) + (p.y()+0.70)*cos(angle) - 0.70 );

   return temp;
}

void SketchWidget::rotateFov(double angle)
{
    Point baseLeft = rotatePoint(Point(0.0, 0.40),23.45 * PI / 180);
    viewDirectionLeft.setX( baseLeft.x()*cos(angle) - (baseLeft.y()+0.53)*sin(angle) );
    viewDirectionLeft.setY( baseLeft.x()*sin(angle) + (baseLeft.y()+0.53)*cos(angle) - 0.53 );
    viewDirectionLeft.limit(0.6);

    Point baseRight = rotatePoint(Point(0.0, 0.40),-23.45 * PI / 180);
    viewDirectionRight.setX( baseRight.x()*cos(angle) - (baseRight.y()+0.53)*sin(angle) );
    viewDirectionRight.setY( baseRight.x()*sin(angle) + (baseRight.y()+0.53)*cos(angle) - 0.53 );
    viewDirectionRight.limit(0.6);
}

void SketchWidget::doRotate(double angle)
{
    //for (double a = 0.0; a < angle; a += 0.01) {
        for (int i=1; i < m_points.size(); i++) {
            m_points[i] = rotatePoint(m_points[i], angle);
        }
    //    updateGL();
        //}
}

void SketchWidget::doTranslate(double distance)
{
    for (int i=1; i < m_points.size(); i++) {
        m_points[i] = translatePoint(m_points[i], distance);
    }
}

void SketchWidget::updateWindow()
{
    updateGL();
}

void SketchWidget::setWindow(Window *w)
{
    mainWindow = w;
}

Mode SketchWidget::getMode()
{
    return mode;
}

void SketchWidget::setMode(Mode m)
{
    mode = m;
}


QVector<Point> SketchWidget::getPoints()
{
    return m_points;
}

QVector<Speed> SketchWidget::getSpeeds()
{
    return m_speeds;
}

void SketchWidget::deletePointIndice(int i)
{
    m_points.remove(i);
    m_speeds.remove(i);
}
